package pl.poznan.put.cs.idss.particlefilter.filter;

import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;

import javax.swing.Timer;

import pl.poznan.put.cs.idss.particlefilter.agent.Agent;
import pl.poznan.put.cs.idss.particlefilter.agent.Particle;
import pl.poznan.put.cs.idss.particlefilter.utils.CopyParticle;

public class ParticleFilter implements ActionListener {

	private int particlesNumber;
	private List<Particle> particles = new ArrayList<Particle>();

	public List<Particle> getParticles() {
		return particles;
	}

	// private Map<Particle, Double> particlesWeights = new HashMap<Particle,
	// Double>();
	private Agent agent;
	private Timer timer;

	public ParticleFilter(Agent agent, int particleNumber)
			throws InterruptedException, IOException {
		this.particlesNumber = particleNumber;
		this.agent = agent;
		agent.setNoise(0.5, 0.5, 5);
		generateParticles();
	}

	public void start() {
		setTimer(new Timer(20, this));
		getTimer().start();
	}

	private void generateParticles() throws InterruptedException, IOException {
		for (int i = 0; i < particlesNumber; i++) {
			Particle particle = new Particle(agent.getWorldMap());
			particles.add(particle);
		}
	}

	public void actionPerformed(ActionEvent arg0) {
		double rotate = agent.getNextRotate();
		double dist = agent.getSpeed();
		agent.move(dist, rotate);
		agent.sense();
		particlesMove(dist, rotate);
		particlesImportanceWeights();
		particlesResample();

	}

	private void particlesResample() {
		List<Particle> newParticles = new ArrayList<Particle>();
		int index = (int) (Math.random() * particlesNumber);
		double beta = 0.0;
		double maxWeight = findMaxWeight();

		for (int i = 0; i < particles.size(); i++) {
			beta += Math.random() * 2.0 * maxWeight;
			while (beta > particles.get(index).getWeight()) {
				beta -= particles.get(index).getWeight();
				index = (index + 1) % particlesNumber;
			}
			Particle p = CopyParticle.copyParticle(particles.get(index));
			newParticles.add(p);

		}
		particles.clear();
		particles.addAll(newParticles);

	}

	/**
	 * wyliczenie wag cząsteczek
	 */
	private void particlesImportanceWeights() {
		double sum = 0;
		for (Particle p : particles) {
			double weight = p.getMeasurementProbability(agent
					.getMeasurementMap());
			p.setWeight(weight);

			sum += weight;
		}
		normalizeImportanceWeights(sum);

	}

	private void normalizeImportanceWeights(double sum) {
		for (Particle p : particles) {
			double normalWeight = p.getWeight() / sum;
			p.setWeight(normalWeight);
		}
	}

	/**
	 * ruch cząteczek
	 */
	private void particlesMove(double dist, double rotate) {
		for (Particle p : particles) {
			p.move(dist, rotate);
		}

	}

	private double findMaxWeight() {
		double max = 0;
		for (Particle p : particles) {
			if (p.getWeight() > max) {
				max = p.getWeight();
			}
		}
		return max;
	}

	public Timer getTimer() {
		return timer;
	}

	public void setTimer(Timer timer) {
		this.timer = timer;
	}

}
